#include <mathematics/Plane.h>
#include <mathematics/AABB.h>
#include <mathematics/Sphere.h>

namespace ne
{
    Plane::Plane()
        : normal(Vector3::UNIT_Y), d(0.0f)
    {
    }

    Plane::Plane(const Plane &p)
        : normal(p.normal), d(p.d)
    {
    }

    Plane::Plane(const Vector3 &vNormal, const real rD)
    {
        redefine(vNormal, rD);
    }

    Plane::Plane(const Vector3 &vNormal, const Vector3 &point)
    {
        redefine(vNormal, point);
    }

    Plane::Plane(const Vector3 &point0, const Vector3 &point1, const Vector3 &point2)
    {
        redefine(point0, point1, point2);
    }

    // a*x + b*y + c*z + d = 0
    Plane::Plane(const real a, const real b, const real c, const real rD)
    {
        redefine(a, b, c, rD);
    }

    void Plane::redefine(const Vector3 &normal, const real d)
    {
        this->normal = normal;
        this->normal.normalize();
        this->d = d;
    }

    void Plane::redefine(const Vector3 &normal, const Vector3 &point)
    {
        this->normal = normal;
        this->normal.normalize();
        d = -normal.dotProduct(point);
    }

    void Plane::redefine(const Vector3 &point0, const Vector3 &point1, const Vector3 &point2)
    {
        Vector3 v1 = point1 - point0;
        Vector3 v2 = point2 - point0;
        normal = v1.crossProduct(v2);
        normal.normalize();
        d = -normal.dotProduct(point0);
    }

    void Plane::redefine(const real a, const real b, const real c, const real d)
    {
        normal = Vector3(a, b, c);
        this->d = d;
        real length = normal.normalize();
        if (length > 0.0f)
        {
            this->d /= length;
        }
    }

    real Plane::normalize()
    {
        return normal.normalize();
    }

    real Plane::getDistance(const Vector3 &point) const
    {
        return normal.dotProduct(point) + d;
    }

    Plane::Side Plane::getSide(const Vector3 &point) const
    {
        real dist = getDistance(point);
        if (dist > 0.0f) return POSITIVE_SIDE;
        if (dist < 0.0f) return NEGATIVE_SIDE;
        return NO_SIDE;
    }

    Plane::Side Plane::getSide(const AABB &a) const
    {
        if (a.isNull()) return NO_SIDE;
        if (a.isInfinite()) return BOTH_SIDE;
        return getSide(a.getCenter(), a.getHalfSize());
    }

    Plane::Side Plane::getSide(const Plane &p) const
    {
        if (normal == p.normal)
        {
            if (d < p.d) return POSITIVE_SIDE;
            if (d > p.d) return NEGATIVE_SIDE;
            return NO_SIDE;
        }
        if (-normal == p.normal)
        {
            if (d < -p.d) return POSITIVE_SIDE;
            if (d > -p.d) return NEGATIVE_SIDE;
            return NO_SIDE;
        }
        return BOTH_SIDE;
    }

    Plane::Side Plane::getSide(const Sphere &s) const
    {
        real dist = getDistance(s.center);
        if (dist > s.radius) return POSITIVE_SIDE;
        if (dist < -s.radius) return NEGATIVE_SIDE;
        return BOTH_SIDE;
    }

    Plane::Side Plane::getSide(const Vector3 &center, const Vector3 &halfSize) const
    {
        real dist = getDistance(center);
        real maxAbsDist = normal.absDotProduct(halfSize);
        if (dist > maxAbsDist) return POSITIVE_SIDE;
        if (dist < -maxAbsDist) return NEGATIVE_SIDE;
        return BOTH_SIDE;
    }

    Vector3 Plane::projectVector(const Vector3 &point) const
    {
        real dist = getDistance(point);
        return point - dist * normal;
    }

    Vector3 Plane::reflectVector(const Vector3 &point) const
    {
        real dist = getDistance(point);
        return point - 2 * dist * normal;
    }

    Plane& Plane::operator = (const Plane &p)
    {
        normal = p.normal;
        d = p.d;
        return *this;
    }

    bool Plane::operator == (const Plane &p) const
    {
        return ((d == p.d) && (normal == p.normal));
    }

    bool Plane::operator != (const Plane &p) const
    {
        return ((d != p.d) || (normal != p.normal));
    }

    // Function for writing to a stream.
    std::ostream& operator << (std::ostream &o, const Plane &p)
    {
        o << "Plane(normal=" << p.normal <<
                 ", d=" << p.d << ")";
        return o;
    }
}
